/********************************************************
 * In this code, the manipulator moves based on the button
 * inputs of the joystick. A variable representing the desired peg
 * is compared to the actual peg. Depending on the comparison, the
 * manipulator either moves up or down. Also moved variables desiredPeg
 * and actualPeg into this function.
 * - Robert at 3/2/11.
 * ******************************************************/

#include "WPILib.h"
#include "Robot2489.h"

#define LOWOUTPEG 1
#define LOWINPEG 2
#define FEEDERSTATION 3
#define MIDOUTPEG 4
#define MIDINPEG 5

void Robot2489::ManipUp() {
	leftSpike->Set(Relay::kForward);
	rightSpike->Set(Relay::kForward);
}
void Robot2489::ManipDown() {
	leftSpike->Set(Relay::kReverse);
	rightSpike->Set(Relay::kReverse);
}
void Robot2489::ManipStop() {
	leftSpike->Set(Relay::kOff);
	rightSpike->Set(Relay::kOff);
}

//No more spinning wheels
/*
void Robot2489::SpinningWheelStop() {
	topWheel->Set(0);
	bottomWheel->Set(0);
}
void Robot2489::SpinningWheelIn() {
	topWheel->Set(-0.7);
	bottomWheel->Set(-0.7);
}
void Robot2489::SpinningWheelOut() {
	topWheel->Set(0.9);
	bottomWheel->Set(0.9);
}

void Robot2489::SpinningWheelRotateDown() {
	topWheel->Set(-0.7);
	bottomWheel->Set(0);
}

void Robot2489::SpinningWheelRotateUp() {
	topWheel->Set(0);
	bottomWheel->Set(-0.7);
}
*/
void Robot2489::ArmUp(){
	//armSpike->Set(Relay::kReverse);
	armMotor->Set(-0.35);
}
void Robot2489::ArmDown(){
	armMotor->Set(0.22);
}
void Robot2489::StopArm(){
	//armSpike->Set(Relay::kOff);
	armMotor->Set(0);
}

//Commented until potentiometer works
/*
void Robot2489::MoveArm(double toAngle){
	if (toAngle > potentiometer->GetValue())		
	{
		while (toAngle > potentiometer->GetValue())
		{
			screen->PrintfLine(DriverStationLCD::kUser_Line1, "potentiometer value = %f", potentiometer->GetValue());
			ArmUp();
		}
	}
	if (toAngle < potentiometer->GetValue())
	{
		while (toAngle < potentiometer->GetValue())
		{
			screen->PrintfLine(DriverStationLCD::kUser_Line1, "potentiometer value = %f", potentiometer->GetValue());
			ArmDown();
		}
	}
	StopArm();
}

void Robot2489 :: ArmAngle(){
	potentiometer->GetValue();
}
*/

void Robot2489::ManipMove(){
	
	onLowOutSwitch = !lowOutSwitch->Get();
	onLowInSwitch = !lowInSwitch->Get();
	onMidOutSwitch = !midOutSwitch->Get();
	onMidInSwitch = !midInSwitch->Get();
	onFeederSwitch = !feederSwitch->Get();
	
	bool manipUp = manipulator1->GetRawButton(3);
	bool manipDown = manipulator1->GetRawButton(2);
	
	int MANIPSTATE;

	if (onLowOutSwitch)
		MANIPSTATE = LOWOUTPEG;
	else if (onLowInSwitch)
		MANIPSTATE = LOWINPEG;
	else if (onFeederSwitch)
		MANIPSTATE = FEEDERSTATION;
	else if (onMidOutSwitch)
		MANIPSTATE = MIDOUTPEG;
	else if (onMidInSwitch)
		MANIPSTATE = MIDINPEG;
	else
		MANIPSTATE = 0;

	if (manipUp && MANIPSTATE!=0 && MANIPSTATE != 5)
		manipDesiredPeg = MANIPSTATE+1;
	else if (manipDown && MANIPSTATE!=0 && MANIPSTATE !=1)
		manipDesiredPeg = MANIPSTATE-1;
	else
		manipDesiredPeg = MANIPSTATE;
	
	manipDirection = MANIPSTATE-manipDesiredPeg;
	
	if (manipDirection<0)
		ManipDown();
	else if (manipDirection>0)
		ManipUp();
	else
		ManipStop();
}

void Robot2489::Manipulator() 
{
	
	/****************************************
	 *The button truths are assigned to the value of the button.
	 Assign values to the variables based on joystick.
	 ****************************************/
	manipvalue = manipulator1->GetY();
	lowOutButton = manipulator1->GetRawButton(6);
	lowInButton = manipulator1->GetRawButton(7);
	feederButton = manipulator1->GetTrigger();
	midOutButton = manipulator1->GetRawButton(10);
	midInButton = manipulator1->GetRawButton(11);
	
	/******************************************
	 * The switch values are assigned to the value of the switch.
	 * Assign values to the variables based on switch.
	 * ****************************************/
	onLowOutSwitch = lowOutSwitch->Get();
	onLowInSwitch = lowInSwitch->Get();
	onMidOutSwitch = midOutSwitch->Get();
	onMidInSwitch = midInSwitch->Get();
	onFeederSwitch = feederSwitch->Get();

	//show which switch is true on the driverstation.
	screen->PrintfLine(DriverStationLCD::kUser_Line1, "Height = %d", manipActualPeg);
	screen->UpdateLCD();

	
	//Button value setting theoretical position;
	if (lowOutButton) //The desired position is now the low out peg.
		manipDesiredPeg = LOWOUTPEG;
	else if (lowInButton) //The desired position is now the low in peg.
		manipDesiredPeg = LOWINPEG;
	else if (feederButton) //The desired position is now the feeder station.
		manipDesiredPeg = FEEDERSTATION;
	else if (midOutButton) //The desired position is now the middle out peg.
		manipDesiredPeg = MIDOUTPEG;
	else if (midInButton) //The desired position is now the middle out peg.
		manipDesiredPeg = MIDINPEG;
	
	//switch values determining actual position.
	if (onLowOutSwitch) //The actual position is at the low out peg.
		manipActualPeg = LOWOUTPEG;
	else if (onLowInSwitch) //The actual position is at the low in peg.
		manipActualPeg = LOWINPEG;
	else if (onFeederSwitch)//The actual position is at the feeder peg.
		manipActualPeg = FEEDERSTATION;
	else if (onMidOutSwitch) //The actual position is at the mid out peg.
		manipActualPeg = MIDOUTPEG;
	else if ((onMidInSwitch)) //The actual position is at the mid in peg.
		manipActualPeg = MIDINPEG;
	
	//comparing button input to actual switch.
	if (manipDesiredPeg>manipActualPeg)
	{//if the wanted position is higher than the actual position.
		ManipUp();
		manipDirection = manipActualPeg + 1;//The manipulator is going up.
	}
	else if (manipDesiredPeg<manipActualPeg) 
	{//if the wanted position is lower than the actual position.
		ManipDown();
		manipDirection = manipActualPeg - 1;//The manipulator is going down.
	}
	
	if(manipDesiredPeg == manipActualPeg && manipDirection > manipActualPeg && !(onLowOutSwitch||onLowInSwitch||onFeederSwitch||onMidOutSwitch||onMidInSwitch))
	{//if person wants previous position in middle of going up.
		ManipDown();
		if(onLowOutSwitch)
		{
			ManipStop();
		}
	}
	else if(manipDesiredPeg == manipActualPeg && manipDirection < manipActualPeg && !(onLowOutSwitch||onLowInSwitch||onFeederSwitch||onMidOutSwitch||onMidInSwitch))
	{//if person wants previous position in middle of going down.
		ManipUp();
		if(onMidInSwitch)
		{
			ManipStop();
		}
	}
	
	if(manipDesiredPeg==manipActualPeg && (onLowOutSwitch||onLowInSwitch||onFeederSwitch||onMidOutSwitch||onMidInSwitch))
	{//if the switch in the same level as position is pressed.
		ManipStop();
	}
}
